
#pragma once
#include "CML.h"
#include"QTimer.h"
//#include "Action.h"
//#include "Filter.h"
//#include "TorqueControll.h"
#include "CML_Threads.h"
//#include "CML_Utils.h"
#include "CML_Amp.h"
#include "windows.h"
#include<iostream>
#include<cstdio>
#include<vector>
#include<math.h>
#include<fstream>

#define PI 3.141592653589793238462643383279
#define CACTION_HASH_SIZE 10
#define DEFAULT_TIMER_PERIOD 10

CML_NAMESPACE_USE();

class CAction;

class CSeaAmp :
	public Amp, public QObject
{
	//Q_OBJECT
	public slots:
		void TimerOutEvent();

public:

	CSeaAmp()
	{
		int i;
		timerPeriod = DEFAULT_TIMER_PERIOD; 
		for(i=0;i<CACTION_HASH_SIZE;i++){
			hash[i]=NULL;
		}
	}
	CSeaAmp( CanOpen &co, int16 nodeID, int tp );
	CSeaAmp( CanOpen &co, int16 nodeID, AmpSettings &settings, int tp );
	~CSeaAmp();

	const Error *Init( CanOpen &co, int16 nodeID, int tp );
	const Error *Init( CanOpen &co, int16 nodeID, AmpSettings &settings, int tp );

	void InitTimer();

	void SetPosMotor( double pm );
	double GetPosMotor( void ) { return posMotor; }

	void SetVelMotor( double vm );
	double GetVelMotor( void ) { return velMotor; }

	void SetAccMotor( double am );
	double GetAccMotor( void ) { return accMotor; }
	
	void SetPosLoad( double pl );
	double GetPosLoad( void ) { return posLoad; }

	void SetVelLoad( double vl );
	double GetVelLoad( void ) { return velLoad; }
	double *GetVelLoadPtr( void ) { return &velLoad; }

	void SetAccLoad( double al );
	double GetAccLoad( void ) { return accLoad; }

	void SetPosSpringZero( double psz ){posSpringZero = psz;}

	double GetPosSpring( void ) { return posSpring; }
	double GetVelSpring( void ) { return velSpring; }
	double GetAccSpring( void ) { return accSpring; }

	void SetTimerPeriod( int tp ) { timerPeriod = tp; }
	double GetTimerPeriod( void ) { return timerPeriod; }

	void SetForceSet( double fs );
	double GetForceSet( void ) {return forceSet;}

	void SetPositionSet( double ps );
	double GetPositionSet( void ) {return positionSet;}

	void SetVelocitySet( double vs );
	double GetVelocitySet( void ) {return velocitySet;}

	void SetVelForceSet( double fvs );
	double GetVelForceSet( void ) {return velForceSet;}

	void SetVelPositionSet( double vps );
	double GetVelPositionSet( void ) {return velPositionSet;}

	void SetVelVelocitySet( double vvs );
	double GetVelVelocitySet( void ) {return velVelocitySet;}

	double GetForceMeasure( void ) {return forceMeasure = posSpring * elasticCoefficient;}

	double GetVelForceMeasure( void ) {return velForceMeasure = velSpring * elasticCoefficient;}

	void SetElasticCoefficient( double ec ) { elasticCoefficient = ec; }
	double GetElasticCoefficient( void ) {return elasticCoefficient;}

	void SetTimeInterval( double ti ) { timeInterval = ti; }
	double GetTimeInterval( void ) {return timeInterval;}

	void SetAccForceSet(double accforceset);
	double GetAccForceSet(void){ return AccForceSet; }


	QTimer *timer;

protected:
	bool EnableAction( CAction *action );

	bool DisableAction(  CAction *action );



private:

	int timerPeriod;


	CAction **searchHash( int id );

	 CAction *hash[ CACTION_HASH_SIZE ];

	Mutex seaHashMutex;

	Mutex dataMutex;

	double posMotor;
	double velMotor;
	double accMotor;

	double posLoad;
	double velLoad;
	double accLoad;

	double posSpring;
	double posSpringZero;
	double velSpring;
	double accSpring;

	double elasticCoefficient;

	double positionSet;
	double velPositionSet;

	double velocitySet;
	double velVelocitySet;

	double forceSet;
	double velForceSet;

	double forceMeasure;
	double velForceMeasure;
 
	double timeInterval;

	double AccForceSet;

	friend class CAction;
};

